using System;
using System.Collections.Generic;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Diagnostics;

namespace XNALibrary
{
    static class Util
    {

        //
        //  Calculate the angle between tw vectors.
        //	The returned angle is between 0 and PI (in radians).
        //

        public static float Angle (Vector3 v1, Vector3 v2)
        {
	        float norm1, norm2, angle;
    
	        norm1 = v1.Length();
	        norm2 = v2.Length();

	        if (norm1 > 0.0f && norm2 > 0.0f)
		        angle = (float)Math.Acos (Vector3.Dot(v1, v2) / (norm1 * norm2));
	        else
		        angle = -1.0f;

	        return angle;
        }

        //
        //	Make a rotation matrix that rotates v1 into v2 direction
        //

        public static Matrix CreateRotate (Vector3 v1, Vector3 v2)
        {
            Matrix m = Matrix.Identity;

	        Vector3 p;	// the rotation axis
	        float angle;	// the rotation angle

	        angle = Angle (v1, v2);
	        if (angle > 0.0f)	// v1 and v2 are not in the same direction;
	        {
		        Vector3.Cross (ref v1, ref v2, out p);
                if (p.Length() > 0.0f)
                {
                    Quaternion q = Quaternion.CreateFromAxisAngle(p, angle);
                    m = Matrix.CreateFromQuaternion(q);
                }
                else			// v1 and v2 are colinear but in opposite direction
                {	// Find a non-zero vertor perpendicular to v1
                    if (v1.X != 0.0f || v1.Y != 0.0f)
                    {
                        p.X = -v1.Y;
                        p.Y = v1.X;
                        p.Z = 0.0f;
                    }
                    else
                    {
                        p.X = 1.0f;
                        p.Y = p.Z = 0.0f;
                    }
                    // Make a rotation matrix (180 degrees around p)
                    Quaternion q = Quaternion.CreateFromAxisAngle(p, (float)Math.PI);
                    m = Matrix.CreateFromQuaternion(q);
                }
	        }

            return m;
        }

        //
        //	Make a 4X4 matrix that transforms p1 to q1 and p2 to q2
        //
        public static Matrix CreateLineToLineTransform (Vector3 p1, Vector3 p2,
            Vector3 q1, Vector3 q2)
        {
            Vector3 vp, vq;
            float np, nq, s;
	        Matrix m, m1;

            vp = p2 - p1;
            vq = q2 - q1;

	        m = CreateRotate (vp, vq);

	        np = vp.Length();
	        nq = vq.Length();
	        if (np > 0.0f) 
                s = nq / np;
	        else 
                s = 1.0f;

	        // Compute the required matrix.
	        Matrix.CreateTranslation (ref q1, out m1);
	        m1.M11 = s;
            m1.M22 = s;
            m1.M33 = s;
	        m *= m1;
            p1 = -p1;
	        Matrix.CreateTranslation(ref p1, out m1);
            m *= m1;

            return m;
        }
    }
}
